#include "RobotHandEyeCalibrationSimulator.hpp"

using namespace std;

RWSPlugin::RWSPlugin(): RobWorkStudioPlugin("RobotHandEyeCalibrationSimulator", QIcon(":/pa_icon.png"))
{
	setupUi(this);
}

RWSPlugin::~RWSPlugin() {}

void RWSPlugin::initialize()
{
	log().info() << "initializing...\n";

	_workCell = rw::loaders::WorkCellFactory::load("E:/Code/RH-AROL/catkin_ws/src/arvp/components/workcell_scene/arvp/arvp.handeye.wc.xml");
	getRobWorkStudio()->setWorkcell(_workCell);
	_state = _workCell->getDefaultState();
	getRobWorkStudio()->stateChangedEvent().add(boost::bind(&RWSPlugin::stateChangedListener, this, _1), this);
	
	//find device
	_deviceRobot = _workCell->findDevice("UR");
	if (_deviceRobot == NULL) log().info() << "Device is not found.\n";
	else log().info() << "Found the device:" << _deviceRobot->getName() << "\n";

	//camera setup
	_cameraFrame = _workCell->findFrame("Hand-Eye-Sim");

	connect(shoot, SIGNAL(clicked()), this, SLOT(btnClicked()));
	connect(calibrate, SIGNAL(clicked()), this, SLOT(btnClicked()));
	connect(save, SIGNAL(clicked()), this, SLOT(btnClicked()));
	connect(setup, SIGNAL(clicked()), this, SLOT(btnClicked()));

	//update robot scene in RWS
	_timer1 = new QTimer(this);
	connect(_timer1, SIGNAL(timeout()), this, SLOT(updateScene()));
	_timer1->start(50);

	shoot->setDisabled(true);
	save->setDisabled(true);
	_imageNumber = 0;
}

void RWSPlugin::open(rw::models::WorkCell* workcell)
{
	log().info() << "Successful open the scene.\n";
}

void RWSPlugin::close() {}

void RWSPlugin::btnClicked()
{
	QObject *obj = sender();
	
	if (obj == shoot)
	{
		log().info() << "Robot config: "<< _robotConfig << "\n";
		//get the image as a RW image
		_frameGrabber->grab(_cameraFrame, _state);
		const rw::sensor::Image& image = _frameGrabber->getImage();

		// Convert to OpenCV image
		cv::Mat res(image.getHeight(), image.getWidth(), CV_8UC3);
		res.data = (uchar*)image.getImageData();
		//the GLFrameGrabber gets 8bit RGB 3-channel color image (Standard OpenGL), 
		//while OpenCV use BGR 3-channel color image (Standard OpenCV)
		cv::cvtColor(res, res, CV_RGB2BGR); 

		// Flip and push image
		cv::Mat imflip;
		cv::flip(res, imflip, 0);
		_image = imflip;

		//cv::namedWindow("grab image", cv::WINDOW_AUTOSIZE);
		//cv::imshow("grab image", _image);
		char imageName[50];
		sprintf(imageName, "image%05d.bmp", _imageNumber);
		std::string str(imageName);
		cv::imwrite(str, _image);

		rw::math::Transform3D<double> base2tcp = _deviceRobot->baseTend(_state);
		_pose.push_back(base2tcp);

		_imageNumber++;

		save->setEnabled(true);
	}
	else if (obj == setup)
	{
		log().info() << "Robot config: " << _robotConfig << "\n";
		setupCamera();
		rw::math::Q initQ(6, -0.818, -0.846, -1.338, -1.917, 1.288, 0);
		_deviceRobot->setQ(initQ,_state);
		shoot->setEnabled(true);
	}
	else if (obj == save)
	{
		FILE * pFile;
		pFile = fopen("myfile.txt", "w");
		int imageCount = _pose.size();
		for (int i = 0; i < imageCount; i++)
		{
			fprintf(pFile, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", 
				_pose[i](0, 0), _pose[i](0, 1), _pose[i](0, 2), _pose[i](0, 3),
				_pose[i](1, 0), _pose[i](1, 1), _pose[i](1, 2), _pose[i](1, 3),
				_pose[i](2, 0), _pose[i](2, 1), _pose[i](2, 2), _pose[i](2, 3));
		}
		fclose(pFile);
	}
	else if (obj == calibrate)
	{
		log().info() << "Robot config: " << _robotConfig << "\n";
	}
}

void RWSPlugin::setupCamera()
{
	if (_cameraFrame != NULL)
	{
		if (_cameraFrame->getPropertyMap().has("Camera"))
		{
			// Read the dimensions and field of view
			double fovy;
			int width, height;
			std::string camParam = _cameraFrame->getPropertyMap().get<std::string>("Camera");
			std::istringstream iss(camParam, std::istringstream::in);
			iss >> fovy >> width >> height;
			// Create a frame grabber
			_frameGrabber = new rwlibs::simulation::GLFrameGrabber(width, height, fovy);
			rw::graphics::SceneViewer::Ptr gldrawer = getRobWorkStudio()->getView()->getSceneViewer();
			_frameGrabber->init(gldrawer);
		}
	}
}

void RWSPlugin::stateChangedListener(const rw::kinematics::State& state)
{
	_state = state;
}

void RWSPlugin::updateScene()
{
	rw::kinematics::State tempState = _state;
	_robotConfig = _deviceRobot->getQ(_state);
	getRobWorkStudio()->setState( tempState );
}

#if RWS_USE_QT5
Q_PLUGIN_METADATA(IID "dk.sdu.mip.Robwork.RobWorkStudioPlugin/0.1")
#else
Q_EXPORT_PLUGIN2(RWSPlugin, RWSPlugin);
#endif